
import array

def calculateChecksum(*data):
	chksum = 0
	for x in data:
		chksum ^= x
	chksum &= 0x7f
	return chksum

def get8bitPositionMoveMessage(torq, servoId, targetPos):
	data = array.array('B')
	data.append(0xff)
	byte2 = torq << 5 | servoId
	byte3 = targetPos
	data.append(byte2)
	data.append(byte3)
	data.append( calculateChecksum(byte2, byte3) )
	return data

def get10bitPositionMoveMessage(torq, servoId, targetPos):

	if targetPos < 4:
		targetPos = 4

	if targetPos > 1016:
		targetPos = 1016

	data = array.array('B')
	data.append(0xff)
	byte2 = 7 << 5
	byte3 = 200
	byte4 = servoId
	byte5 = torq
	byte6 = targetPos >> 7 & 0x07
	byte7 = (targetPos) & 0x7f
	byte8 = calculateChecksum(byte2, byte3, byte4, byte5, byte6, byte7)
	data.append(byte2)
	data.append(byte3)
	data.append(byte4)
	data.append(byte5)
	data.append(byte6)
	data.append(byte7)
	data.append(byte8)
	return data

def get10bitPositionReadMessage(servoId):
	data = array.array('B')
	data.append(0xff)
	byte2 = 7 << 5
	byte3 = 201
	byte4 = servoId
	byte5 = byte4
	byte6 = calculateChecksum(byte2, byte3, byte4, byte5)
	data.append(byte2)
	data.append(byte3)
	data.append(byte4)
	data.append(byte5)
	data.append(byte6)
	return data	
